Periodic-Feedback Motion Planning in Belief Space for Nonholonomic and/or Nonstoppable Robots

In roadmap-based methods, such as the Probabilistic Roadmap Method (PRM) in deterministic environments or the Feedback-based Information RoadMap (FIRM) in partially observable probabilistic environments, a stabilizing controller is needed to guarantee node reachability in state or belief space. In the Linear Quadratic Gaussian-based (LQG-based) instantiation of FIRM, it has been shown that for controllable linear systems, belief node reachability can be achieved using a stationary LQG controller. However, for nonholonomic and/or non-stoppable systems (whose velocity cannot be zero), belief reachability is a challenge. In this paper, we propose a novel method based on periodic trajectories, in which instead of stabilizing the belief to a predefined point the belief is stabilized to a periodic path in the belief space through which it is driven into predefined belief nodes. Therefore, the belief-node reachability is achieved and an instantionation of the FIRM framework that can handle the nonholonomic and/or non-stoppable systems is introduced. While taking obstacles into account, this method serves as an offline POMDP solver for motion planning in belief space. It is a query-independent solution, and preserves the optimal substructure property, required in dynamic programming solvers. Experiments illustrate the planning procedure on a unicycle model.